/********************************************************************************
*  Project   		: FIRST Motor Controller
*  File Name  		: SimpleTracker.cpp        
*  Contributors 	: ELF
*  Creation Date 	: Oct 17, 2008
*  Revision History	: Source code & revision history maintained at sourceforge.WPI.edu   
*  File Description	: Demo program showing simple color tracking
*/
/*----------------------------------------------------------------------------*/
/*        Copyright (c) FIRST 2008.  All Rights Reserved.                     */
/*  Open Source Software - may be modified and shared by FRC teams. The code  */
/*  must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib. */
/*----------------------------------------------------------------------------*/

#include <iostream.h>
#include "math.h"
#include <taskLib.h>
#include <algorithm>

#include "AxisCamera.h" 
#include "BaeUtilities.h"
#include "FrcError.h"
#include "TrackAPI.h" 
#include "PCVideoServer.h"
#include "WPILib.h"
#include "Utility.h"

#define WHEEL_CIRCUMFERENCE (18.84*0.0254) // convert inches to m
#define ENCODER_TICKS_PER_REVOLUTION 1000 
#define ACCELEROMETER_ZERO 1.5 // minimum voltage on accelerometer: 1.5 V at freefall
#define ACCELEROMETER_V_PER_G 0.3

#define ENCODER_SPEED_DEADBAND 0 //we'll need to experiment with this depending on how much velocity varies
#define ENCODER_SPEED_INCREMENT 0.01

#define MIN_PARTICLE_TO_IMAGE_PERCENT 0.25		// target is too small
#define MAX_PARTICLE_TO_IMAGE_PERCENT 100.0		// target is too close
/**
 * This is a demo program showing the use of the color tracking API. 
 * It uses the SimpleRobot class as a base of a robot application that will automatically call 
 * your Autonomous and OperatorControl methods at the right time as controlled by the switches
 * on the driver station or the field controls. Autonomous mode tracks color, assuming a 
 * camera on a fixed mount. 
 */

// the bit after the colon means this class is derived from SpeedController
// it inherits everything in SpeedController, including all the virtual functions
// the virtual functions are ones that can be replaced by our code, as we do here:
class EncoderSpeedController : public SpeedController {
	// everything below can't be used by anything but this class
	// this is sorta pointless, but its main purpose is to indicate to other programmers
	//  what functions should be called externally, and what ones they shouldn't screw with
private:
	Encoder *encoder;
	SpeedController *speedController;
	float jaguar_speed;
	
	
	double calculateEncoderSpeed() {
		// in this function we don't have to worry about time
		// because the FPGA returns pulse width for us
		
		// returns meters per second
		if (encoder->GetPeriod() == 0 || encoder->GetPeriod() > 0.1)
			return 0;
		return (WHEEL_CIRCUMFERENCE/ENCODER_TICKS_PER_REVOLUTION)
				/ (encoder->GetPeriod());
	}

	// the functions below can be used outside this class
public:
	EncoderSpeedController(SpeedController *controller, Encoder *enc) {
		speedController = controller;
		encoder = enc;
		jaguar_speed = 0.0;
	}
	void Set(float speed) {
		// TODO!
		// this function must be called to update the motors
		// assume speed is in meters per second
		if ((calculateEncoderSpeed() - speed) > ENCODER_SPEED_DEADBAND) //if actual speed is greater than intended speed
			jaguar_speed -= ENCODER_SPEED_INCREMENT; //does -= work to decrease?
		else if ((speed - calculateEncoderSpeed()) > ENCODER_SPEED_DEADBAND) // if actual speed is less than intended speed
			jaguar_speed += ENCODER_SPEED_INCREMENT;
		
		if (jaguar_speed > 1)
			jaguar_speed = 1;
		else if (jaguar_speed < -1)
			jaguar_speed = -1;
		
		speedController->Set(jaguar_speed);
	}
	float Get(void) {
		// return float for compatablility with SpeedController ?
		return (float)calculateEncoderSpeed();
	}
};

// TODO: the following class fails at life, the universe, and everything
// it is a hack and needs to be fixed up after EncoderSpeedController is working
class AntiSlipRobotDrive {
public:
	RobotDrive *robotDrive;
	Encoder *encoder;
	Accelerometer *accelerometer;
	Timer timer;
	double lastSpeed;
	double lastTime;
	
	double maxAcceleration;
	
	AntiSlipRobotDrive(SpeedController *leftMotor, SpeedController *rightMotor,
			float sensitivity, Accelerometer *accelerometer) {
		robotDrive = new RobotDrive(leftMotor, rightMotor, sensitivity);
		this->accelerometer = accelerometer;
		timer.Reset();
		timer.Start();
		lastTime = 0;
		maxAcceleration = 0.1;
	}
	void Drive(float requestedSpeed, float direction) {
		// timekeeping
		double currentTime = timer.Get(); // microseconds
		double timeDifference = currentTime - lastTime;
		lastTime = currentTime;
		// figure out our acceleration
		// speed is in meters per second
		//double speed = (WHEEL_CIRCUMFERENCE/ENCODER_TICKS_PER_REVOLUTION)
		//	/ (encoder->GetPeriod());
		double speed = requestedSpeed;
		// acceleration is in meters per second per second
		double wheelAcceleration = (speed - lastSpeed) / timeDifference;
//		double realAcceleration = accelerometer->GetAcceleration() * 9.81;
//		bool wheelIsSlipping;
//		if (wheelAcceleration > realAcceleration) {
//			wheelIsSlipping = true;
//		} else {
//			wheelIsSlipping = false;
//		}
//		if (wheelIsSlipping) {
//			maxAcceleration -= 0.1 * timeDifference;
//		} else {
//			maxAcceleration += 0.1 * timeDifference;
//		}
		double maxSpeed = lastSpeed + (timeDifference * maxAcceleration);
		if (speed > maxSpeed)
			speed = maxSpeed;
		// WRONG
		robotDrive->Drive(speed, direction);
		lastSpeed = speed;
	}
//	void TankDrive(Joystick *leftStick, Joystick *rightStick) {
//		double currentTime = timer.Get(); // microseconds
//		double timeDifference = currentTime - lastTime;
//		lastTime = currentTime;
//		
//		double speed;
//		if (leftStick->GetY() > rightStick->GetY())
//			speed = leftStick->GetY();
//		else
//			speed = rightStick->GetY();
//		double maxSpeed = lastSpeed + (timeDifference * maxAcceleration);
//		//robotDrive->TankDrive(speed, direction);
//				lastSpeed = speed;
//	}
	~AntiSlipRobotDrive() {
		delete robotDrive;
	}
	void ArcadeDrive(Joystick *stick) {
		Drive(stick->GetY(), stick->GetX());
	}
};


class SimpleTracker : public SimpleRobot {
	Joystick *rightStick; 		// joystick 1 (arcade stick or right tank stick)
	Joystick *leftStick; 		// joystick 2 (tank left stick)
	
	Jaguar *leftJaguar;
	Jaguar *rightJaguar;
	
	//EncoderSpeedController *testEncoderController;
	
	Accelerometer *xAccel;
	
	Solenoid *solenoids[8];
	
	DriverStation *ds; 			// driver station object
//DashboardDataFormat dashboardDataFormat;
	TrackingThreshold tdata; 	// image data for tracking
	AntiSlipRobotDrive *robotDrive;

	Encoder *encoder;
public:
	/**
	 * Constructor for this robot subclass.
	 * Create an instance of a RobotDrive with left and right motors plugged into PWM
	 * ports 1 and 2 on the first digital module.
	 */
	SimpleTracker(void) {
		ds = DriverStation::GetInstance();
		// the following two are swapped because it's easier
		// than physically moving cables - that burns calories
		leftJaguar = new Jaguar(1);
		rightJaguar = new Jaguar(2);
		
		encoder = new Encoder(1,2);
		encoder->Start();
		
		//testEncoderController = new EncoderSpeedController(leftJaguar, encoder);
		
		xAccel = new Accelerometer(1);
		xAccel->SetZero(ACCELEROMETER_ZERO);
		xAccel->SetSensitivity(ACCELEROMETER_V_PER_G);
		
		for(int i=0;i<7;i++) {
			solenoids[i] = new Solenoid(i+1);
		}
		
		robotDrive = new AntiSlipRobotDrive(leftJaguar, rightJaguar, 0.1);	// robot will use PWM 1 and 2 for drive motors	
		leftStick = new Joystick(1);
		rightStick = new Joystick(2); 			// create the joysticks
//
		SetDebugFlag(DEBUG_SCREEN_ONLY);		// dprintf output goes to terminal/console
		
		/* start the CameraTask	 */
		if (StartCameraTask(10, 0, k160x120, ROT_0) == -1) {
			dprintf( LOG_ERROR,"Failed to spawn camera task; Error code %s", 
					GetVisionErrorText(GetLastVisionError()) );
		}
		Wait(2.0);
		PCVideoServer pc;
		pc.Start();
//
//		// values for tracking a target - may need tweaking in your environment
//		// see TrackAPI.cpp for threshold values
		tdata = GetTrackingData(GREEN, FLUORESCENT);
//		tdata.luminance.maxValue = 255;
//		tdata.luminance.minValue = 0;
//		tdata.hue.minValue = 0;
//		tdata.hue.maxValue = 255;
//		tdata.saturation.minValue = 0;
//		tdata.saturation.maxValue = 255;
		
		/* stop the watchdog if debugging  */
		GetWatchdog().SetEnabled(false);
		//GetWatchdog().SetExpiration(200);
	}

	void Autonomous(void) {
//		GetWatchdog().Feed();
//		
		ParticleAnalysisReport par; // particle analysis reports

		while (IsAutonomous()) {

//			/* this simple test will drive toward GREEN 
//			 * drive will last until autonomous terminates
//			 */	
//			// delay in loop. For frame rate of 10 fps, a new image is available every 100 ms.
//			// put the Wait at the beginning vs end of loop because the first time through
//			// the camera is still initializing.
//			Wait(.05); 
//			GetWatchdog().Feed();
			if (FindColor(IMAQ_HSL, &tdata.hue, &tdata.saturation, &tdata.luminance, &par)
					&& par.particleToImagePercent < MAX_PARTICLE_TO_IMAGE_PERCENT
					&& par.particleToImagePercent > MIN_PARTICLE_TO_IMAGE_PERCENT) {
				// drive toward color
				//robotDrive->Drive((float)1, (float)par.center_mass_x_normalized);
//				ShowActivity("%s     found: follow x value: %f", tdata.name, par.center_mass_x_normalized);
			} else {
				//robotDrive->Drive(0.0, 0.0); // stop robot
//				ShowActivity("%s NOT found. particleToImagePercent: %f",
//								tdata.name, par.particleToImagePercent);
			}
		} // end while

//		myRobot->Drive(0.0, 0.0); // stop robot
	} 

	/**
	 * Runs the motors under driver control with either tank or arcade steering selected
	 * by a jumper in DS Digin 0. 
	 */
	void OperatorControl(void) {
		while ( IsOperatorControl() )
		{
			//printf("hello\n");

//			dashboardDataFormat.PackAndSend();
//			GetWatchdog().Feed();
//			myRobot->ArcadeDrive(rightStick); // drive with arcade style (use right stick)
			//robotDrive->TankDrive(leftStick, rightStick);
			robotDrive->ArcadeDrive(leftStick);
			SetRIOUserLED(encoder->Get() & 0x100);
			solenoids[0]->Set(xAccel->GetAcceleration() >= 1.0);
			//testEncoderController->Set(0.5);
		}
	} 
};

// entry point is FRC_UserProgram_StartupLibraryInit
START_ROBOT_CLASS(SimpleTracker)
;

